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ABSTRACT 

An efficient formulation is presented for a sub-class of multi-body dynamics problems that involve a six 
degree-of-freedom base body and a chain of N rigid linkages connected in series by single degree-of-freedom 
revolute joints. This general method is particularly well suited for simulations of spacecraft dynamics and 
control that include the modeling of an orbiting platform with or without internal degrees of freedom such 
as reaction wheels, dampers, and/or booms. In the present work, particular emphasis is placed on dynamic 
simulation of multi-linkage robotic manipulators . The differential equations of motion are explicitly given 
in terms of linear and angular momentum states , which can be evaluated recursively along a serial chain of 
linkages for an efficient real-time solution on par with the best of the 0 (N 3 ) methods. 

INTRODUCTION 

The solution to multi-body dynamics problems, and specifically real-time robotic simulation, has remained 
an intellectually and computationally challenging problem for the past 35 years. A multitude of formulations 
of the dynamic simulation (forward dynamics) problem exist in the literature, each with its own particular 
strengths. Traditionally, the methods have been grouped into formulations with dependent coordinates and 
independent coordinates . The former requires solutions of m ordinary differential equations (ODE) along 
with an associated n number of algebraic equations of constraint, where the total number of degrees of 
freedom ( N ) is equal to m — n. The dependent coordinate methods tend to be very general but more difficult 
to solve and are the basis of commercial packages such as Adams® or Dads®. Formulations based on a 
minimal set of independent coordinates are more specific to an application, but can be correspondingly more 
efficient. The methods used to derive the equations of motion also vary considerably (e.g. Euler-Lagrange[l], 
D’Alembert [2] and Newton-Euler[3]) but ultimately the resulting equations are equivalent^]. Further delin- 
eation between methods can be made based on whether the final treatment of the equations of motion are 
explicit or recursive. Explicit solutions (sometimes referred to as closed-form [5] — a misnomer since numeri- 
cal integration is still required for a time-domain solution) tend to be exceedingly complex expressions, even 
for a fairly small number of degrees-of-freedom, and typically are generated using symbolic manipulation pro- 
grams such as Mathematica®, Maple® or Autolev®. Furthermore, the explicit solution is specific to a 
particular problem/configuration and for that reason also has the potential to be the most computationally 
efficient [5,6]. A sub-category of independent coordinate methods, that applies to serial or branched manip- 
ulator systems, uses a recursive algorithm to solve the equations of motion. Recursive solvers are historically 
very fast, especially for the small number of degrees of freedom (N < 10) typical to aerospace applications [7j. 

In the present work, a recursive form applicable to open-loop, chain-like, serial manipulators with rigid 
linkages is considered. The form of the equations presented differs from both the Euler-Lagrange method 
as well as from the traditional Newton-Euler approach [3]. The former method that uses ODE’s derived 
from energy and Lagrange multipliers for elimination of constraints. The latter method solves for relative 
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is assembled via multiple passes of a (forward-backward) recursive inverse dynamics algorithm [10] (i.e., a 


1 



method of obtaining joint torques given the link velocities and accelerations). Instead, the method pre- 
sented here is an extension of work by P. C. Hughes[8] which uses a topology analogous to the direct-path 
method of Ho[9]. This method is written in terms of first-order differential equations for the linear and 
angular momenta. In choosing Hughes’ momentum- state dynamics (MSD) formulation we gain advantages 
over traditional Newton-Euler in being able to: fully couple the translational and rotational dynamics (i.e. 
floating-platform ) in a straightforward and rigorous manner; to include other momentum storage devices 
such as reaction wheels, dampers and gears; and to directly apply environmental forces/moments at arbi- 
trary locations along the serial chain. 

First, for context, a table of notational differences between the present multi-body equations and those 
of Hughes [8] is presented. Next, this paper presents the recursive equations of motion suitable for real-time 
dynamic simulation, provides a low-order (non-recursive) example of the form of the equations, and outlines 
the solution algorithm. Clarification on a method for including disturbances is then discussed followed 
by an example application in the field of space-based robotic control. Lastly, consideration is given to 
computational efficiency, numerical precision, and future extensibility. 


NOTATION 

Since a derivation of the dynamics is not included in this paper, rather the final form of the equations, it is 
important to understand that the underlying development is an extension to the work of P. C. Hughes[8]. 
In general, an effort was made to comply with the notation and style of this reference. Scalars are expressed 
in a medium italic typeface, and 3x1 column 3x3 square matrices are expressed in boldface. A super- 
script cross ( x ) indicates a left-unary operation on a column vector that forms a skew-symmetric matrix, 
and a superscript ( T ) indicates a transpose of a matrix. For matrices , the over-dot notation, ( * ), is used to 
represent the time derivative of a variable with respect to a non-inertial body- fixed frame. In contrast, the 
over-dot on the Gibbsian vectors of Eqs.(l) and Eq.(2), are time derivatives with respect to an inertial frame. 


Table Is Description and Cross-Referencing of Symbols 


Symbol 

Ref. [8] 

Description 

an 

'™m 

G'ran 

attitude (direction cosine) matrix 
that transforms via left multiplica- 
tion a column vector from the n th to 
the vn th frame 

dn 

a 

axis of rotation of the n th joint ex- 
pressed in the n th body frame 

1 * 771,71 

b 

vector expressed in the m th body 
frame pointing from the m th body 
origin to the n th body origin 

C n , c n 

c 

I st mass moment (m n • (r cm ) n ) of 
an individual body (c n ) or the com- 
posite (C n ) of the n th body and all 
bodies outboard, expressed in the n th 
body-fixed frame 

fext ‘E'ext 

L n » x n 

f 

external (composite) force acting on 
the n th body /composite expressed in 
the n th body-fixed frame 

sl xt , Gl Kt 

on 7 n 

g 

external (composite) moment acting 
about the n th body’s origin expressed 
in the n th body-fixed frame 

§71 

gp 

inter-body moments exerted on the 
n th body by the (n— l) th body, about 

n th body frame 


Symbol 

Ref. [8] 

Description 

m n ,M n 

m n 

mass of n th body / composite mass 
of n th body plus all outboard bodies 

hm Hn> 

h n , h 

absolute angular momentum of n th 
body /composite of n th body plus 
outboard bodies, expressed in the n th 
body frame (or projected along a n ) 

jn> Jn 

Jn 

2 nd mass moment of inertia of the n th 
body and the composite of n th body 
plus all outboard bodies, expressed in 
the n th body frame 

Jm,n 

J 12 

“mixed” 2 nd mass moment of inertia 
of the n th and all outboard bodies, 
expressed both in the m th and in the 
n th body frames 

P„ 

P 

composite linear momentum of the 
n th and outboard bodies expressed in 
the n th body frame 

v„ 

V 

velocity of the n th body with respect 
to the inertial frame, expressed in the 
n th body frame 


Up 

inertial angular velocity of n th body 
or, relative scalar component of n th 
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joint axis, expressed in n th frame 
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In some instances it is expedient to depart from Hughes’ notation. One example, vectrices — an invention 
of Hughes for book-keeping the projection of vectors into coordinate frames (and a source of some confusion 
to the unfamiliar) — are used in the derivation but are not needed in this paper’s context. Instead (except for 
Eqs(l,2)), the equations are expressed in matrix form only, with the frame explicitly indicated. Additionally, 
other small changes are made where it is felt it benefits clarity, such as the use of uppercase to imply a com- 
posite property, or one belonging to a collection of bodies — usually the n th body and outboard to the 7V th . 
A cross-reference between the present N-link momentum state dynamics and Hughes (two-body) notation 
is given in Table 1. 


DYNAMICAL EQUATIONS OF MOTION 

In Hughes [8], the familiar Newton-Euler equations of motion for a rigid body are derived from first principles 
and expressed in terms of Gibbsian vectors: 


_p = i, (!) 

h + v„xp = g 0 (2) 

where the linear momentum of the body is p = mjv 0 + _c. x i4> an( l angular momentum about the 
body-fixed reference origin is h Q = x v 0 + J^° w. The only unfamiliar term might be the first mass 
moment vector ( c ), defined by an integral of the mass density function, p(_rj, over the rigid body, 7, as 
_c y = _r^p( r\)dV = m ■ j^ cm . With the simplification that the origin of the rigid-body coordinate system 

is at the center of mass (i.e. c = 0 ), and the notational adjustment that the second mass moment of 
inertia dyadic ( ( J ? ) is expressed as when it is about the center of mass, the equations of motion take a 
more familiar form: 


p = — cj x rav cm + f (3) 

h C m = -U> X Io; + g C m (4) 

where Eqs.(3) and (4) are now expressed in matrix form, with the vectors realized in the non-inertial body- 
fixed frame. Note that these are first order ODEs with momentum variables on the left-hand side and 
velocity variables on the right-hand-side. Hughes[8] also introduces symbols for the generalized momentum 
(?), generalized mass (M), and generalized velocity (V) matrices. For this simplified example, the equation 
that relates the generalized momenta to the generalized velocities — again in matrix form expressed in the 
body-fixed frame — is as follows: 

7 = MV (5) 


’ p 


m • I 

0 


v cm 

_h cm _ 


0 

I 


U) 


The intent is not to over-complicate the classically simple equations for rigid body spacecraft position and 
attitude motion (which are de-coupled by the choice of the center of mass as the origin) but instead to make 
an explicit notational bridge between the method to be presented and the familiar territory of practicing 
engineers — acknowledging that notation is often the largest barrier in the acceptance of a new methodology. 

Returning to the more general case, in which the base coordinate system does not coincide with the 
system center of mass and the translational and rotational equations of momentum are coupled, Hughes[8] 
shows how this straight-forward framework can be extended to several simple, but powerful, multi-body ap- 
plications: a point mass (sliding) damper, a rotor, and an appended rigid body connected to the “base” body 
by a three degree-of-freedom spherical joint. Additionally, Ford and Hall [11] also applied the Hughes frame- 
work to the problem of modeling gimbaled momentum wheels (Control Moment Gyros). To this tool-chest 
of spacecraft component models, the present worK adds an emcient formulation for an appendage consisting 
of a chain of N rigid linkages connected in series by single degree-of-freedom rotational joints. Structures of 
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this nature are often used as robotic manipulators , which is in fact the motivation for this development effort. 


For the purpose of exposition, a four-body (“base” body plus three linkages) example will be presented 
explicitly. Once the form of the equations is made clear, the more general iV-body recursive formula will be 
given. The four-body system configuration is shown in Figure 1. Note that T* is an inertial reference frame, 
and To is the “base” body-fixed frame that is nominally not located at the system center of mass or at the 
base body center of mass. Each linkage has an embedded frame, T n , offset from the frame of the previous 
link (or base) by an outboard translation (b n?n+ i) and associated outboard rotation parameters (quater- 
nion), q™_j_i. The two static configuration parameters, b n and q£ +x , produce an equivalent homogeneous 
transformation matrix between bodies similar to Denavit-Hartenberg[12] parameters common to robotics 
literature. When the rotational degree-of- freedom is aligned with an axis of the link frame (typically the z- 
axis), the simplification can be used to improve computational efficiency. The expression for the generalized 
momentum of the four-body system can be written: 


3> = MV (7) 


Po 


M 0 * I -Cj Cfar -A 2 0 C*a 2 -A^ a 3 


v 0 

Ho 


Cq Jo Jo, i^i Jo, 2^2 Jo, 3^3 


u > 0 

m 

= 

aJCfA? a^Ji )0 a{Jiai a-[Ji, 2 a 2 a iJi,3 a 3 


, .a 

Hi 


^2^2 *^2 a 2 J2,0 a^ J 2 ,l a l a 2 J2 a 2 a 2 J2,3 a 3 


t i a 



agCgcAg a^J^o a 3 J3,i a i a.3J 3}2 a 2 a 3 J3 a 3 


u 3 


where 1 is the 3x3 identity matrix. The associated first order differential equations of motion for the 
momentum states are: 


P 0 = -u; 0 x Po + Fr (9) 

Ho = —uJq H 0 — Vq Po + Gq x * (10) 

Hi = wI& 1 x H 1 +vIa?P 1 + a; gl + &{Gr (H) 

H% = + v^a x P 2 + a T 2 g 2 + ( 12 ) 

#3 = ^sasHa+v^Ps + a^gg+a^GI 5 " (13) 


The most notable difference between Eqs.(8-13) and those of the rigid-body, Eqs.(3-6), are the three addi- 
tional states (i7f , HS;, Hg) representing the composite absolute angular momentum of the n th through N th 
links, about the n ttl link origin (i.e. joint), projected onto the n zri joint's axis of rotation (a n ). Additionally, 
there are three new associated (scalar) angular velocities (a;£), representing the relative angular rate of the 
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n th body with respect to the (n - l) th body about a n . Also, the shift from lowercase to uppercase notation 
is a mnemonic for the fact that these momenta are for a collection of bodies ( n th plus all outboard) which 
are referred to here as composite properties. Finally, the generalized mass matrix is now fully populated — 
indicating that the translational and rotational motion is no longer de-coupled. The application of the 
internal and external forces and moments (g n ,F® xt , G^ xt ) will be covered in a dedicated section of this paper. 

The calculation of the time rate of change of the composite momenta is somewhat complicated by the 
fact that the derivatives depend on the instantaneous momenta of the body and those outboard, but only the 
total system momenta (P 0 , H 0 ) and the scalar components of the momentum about the individual joint axes 
(if“) are propagated as states. The (vectorial) momenta are reconstructed using the following relationships, 
as shown for the four-body case: 


wi = A?u>o+w?ai (14) Vl = A? (v 0 + u; 0 x bo,i) (17) 

u 2 = (15) v 2 = A\ (vi 4- u; x bi |2 ) (18) 

u; 3 = A\uj 2 + u; 3 a 3 (16) v 3 = A% (v 2 4- w^b^) (19) 

P 3 = m 3 v 3 + y 3 x c 3 (20) 

P 2 = m 2 v 2 + c ^2 c 2 +^2^3 (21) 

Pi = mivi + co* Ci + A%P 2 (22) 

Hi = C* A^vq + Ji,o^o H~ Ji^i^i + Ji, 2 a 2 ct;2 + Ji, 33-3^3 (23) 

H 2 = 02^2 Vo + J2,0^0 + J 2 , + J 2 a 2 u;2 + J2,3^-3^ 3 (24) 

Hs = C 3 *A 3 vo + J3,o^o + J 3} iaiu;i + 33 , 28 - 2^2 + J3a 3 a; 3 (25) 


N-body Recursive Equations 

Even from the simple four-body example, the pattern in the equations is evident. A generalized iV-link set 
of equations can be generated from a compact and computationally efficient recursive collection of formulas, 


N 

P 0 = Mo-Ivo-C 0 x u>o-£ASC*a n t4 (26) 

n= 1 
N 

Ho = Cq v 0 4- Jo^o + Jo,n^n^n (27) 

n=l 

p n = m n v n + u; x c n + A£ +1 P n+1 (28) 

N 

H n — C n A n Vo + Jn, 0^0 T ^ ^ ^ where J n,n = (29) 

m = 1 

P = -wJP + Fy* (30) 

Ho = -u; 0 x H 0 -v 0 x P + Gr (31) 

HI = + v^a^P n + a^g n + a^G“* (32) 

and, 

K = aJ,H n (33) 

uj n = ^-Wi+w“a„ (34) 

Vn = KT 1 (v n -i +W x _ 1 b n _ ]>n ) (35) 


where n — 1,2 , ... ,N — 1, AT (zeroth body is the “base” body/frame). All of the recursions begin at the base 
body and work outboard except for the composite linear momentum (P n ) which recurses inboard. 
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MASS PROPERTIES 


Solution of the linear set of equations in Eq.(8) is needed in order to recover the rates used in calculating 
momentum state derivatives. This, in turn, requires computation of elements of the generalized mass matrix, 
M, a non-trivial task. The following recursive equations provide a method for determining the quantities 
needed to populate the generalized mass matrix. 


Composite Mass 


The composite mass is a straight-forward summation of the individual link mass (m n ) with that of the 
outboard masses and which needs to be determined only once for fixed-mass systems. 


{ m n n — N 

m n + M n + 1 n = N - 1, . . . , 1,0 


(36) 


Composite First Mass Moment of Inertia 


The first mass moment for a single body (c n ) is simply the position vector to the n th body’s center of mass 
( v cm n ) scaled by the body’s mass (ra n ). The composite first mass moment is just the equivalent for the 
collection of bodies that includes the n th and all outboard bodies. A recursive formula for generating the 
composite first mass moment — expressed in the n th link frame — starts at the outer-most body as follows: 


{ c n n — N 

c n 4- M n 4 .ib n?n+ i 4- A£ +1 C n+ i n = N — 1 , . . . , 1,0 


(37) 


Second and “Mixed” Mass Moments of Inertia 


Computation of the composite second mass moments of the n th and outboard bodies about the n th body 
origin and expressed in the n tn frame from the mass properties of the individual bodies is facilitated by the 
following recursion relationship: 


Jn 


J n 

j n + 1 J n+1 A£ +1 - (Cn - c n ) x b* n+1 - b^ n+1 A-+ 1 C^ 1 A- +1 


n = N 

n = JV— 0 


(38) 


which was derived using the matrix identity -u x v x — (u T v)I- vu T . Using the same identity, one may write 
a compact expression of the parallel axis theorem for the second mass moment of inertia matrix of a rigid 
body about a point B given its inertia matrix about point A (in the same frame) expressed in matrix form 
as jb ~ 3a+^^a,b^b,a^a,b cX " cX ^b,a^ which ma Y provide some insight regarding the form of Eq.(38). 


The so called “mixed” moments of inertia (J m>n ), as extended here, are derived from a construct of 
Hughes[8j that significantly simplifies the expressions of the composite momenta. Expressing succinctly 
the physical meaning of the mixed moments is not at first straightforward. However, it is clear that the 
mixed moments of inertia capture the coupling between the momentum of a given collection of bodies and 
the momentum of a subset of bodies outboard of a particular “base” body — a sort of transformation of 
the outboard system momentum into the “local” body-fixed frame. Similar to coordinate transformation 
matrices (e.g. mixed mass moment matrices span multiple frames. Quantities that right multiply a 

mixed mass moment matrix, JJ^, need to be realized in the n- frame and vectors or matrices that left multiply 
need to be in the m-frame. One relationship for calculating the mixed mass moments is: 


J m n = AZ J n - b x AI1C* 

Tn,n “ ^rri 7 *' m.n V vn n 


(39) 


The mixed mass moment is not symmetric and J m?n — J^ m can he shown to be true instead. Using Eq.(39), 
an alternate form of Eq.(38b) can be derived. The alternative and more computationally economical recursion 
formula for the second mass moment of inertia is 


Jn — jn "b Jn.n+l*A n _j_j (C n C n ) b njn< j,^ Tl — N 1, . . . , 1, 0 (40) 

where it is evident that the mixed mass moment may also be thought of as a “portion” of the second mass 
moment. 
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FORCES AND MOMENTS 


The dynamical model permits general application of forces or moments to the system. The treatment of the 
force or moment differs slightly if it is from an internal or external source. The appropriate application for 
the two cases is discussed in the subsections that follow. 

Internal Disturbances 

At the total system level (Po,Ho), internal forces and moments are absent from the linear and angular 
momentum equations of motion, due to the action-reaction cancellation (Newton’s Third Law). However, 
at the subsystem level (P n ,H n ), internal forces and torques can appear as in Eqs.(ll-13). 

Specifically for a serial chain of revolutes, only the internal torques, g n , matter (and actually only the 
scalar projection along the joint’s axis of revolution) since there are no translational and only one rotational 
degree-of-freedom for the n th subsystem relative to the (n — l) th subsystem. This inter-body torque might 
represent joint friction or a motor torque. The salient feature is that it acts on the n th body (through the 
n th joint) and must have a corresponding equal-and-opposite torque acting on the ( n — l) th body. Concep- 
tually, the composite system of masses — that includes the n th body and all outboard bodies — experiences 
the torque from the (n — l) t/l body as if it were an external torque, since its source (body n— 1) is external 
to that particular subsystem. Explicitly, g n is the torque on the n th body, expressed in the n-frame, due to 
the (n — l) t/l body. Moving inboard to the (n — l) t/l subsystem calculations, the torque g n now becomes a 
wholly internal action-reaction and so is absent from the state derivative equation for the composite angular 
momentum. 

For a corresponding treatment of an internal force , refer to the example in the section “A System with 
Damping” in reference [8]. 

External Disturbances 

External disturbances may be applied at any location and on any body. In Figure 1 an external force ( f 3 xt ) 
and external torque ( g | xt ) is shown being applied to the last link of the kinematic chain — such as might be 

experienced by an end-effector of a robotic manipulator as it tries to grapple a target — but, as in the case 
of gravity, could also be experienced independently by all of the bodies. To capture the cumulative effect 
simply, we have chosen to define composite forces (F^ xt ) and torques (G^ xt ) that propagate in-board via the 
recursion relations 


TT'ext 

b n ~ 

hr 

hr 

+ K +1 V*nU 

n = N 

n = N — l, . . 

.,1,0 

(41) 

p ext 

fsr 

\sT 

+ r f* f r 

+ r£fr + Apt l G£ 1 + 

n = N 

n = N — 1, . . 

.,1,0 

(42) 


One important point worth mentioning is regarding the point of resolution of external disturbances. Specif- 
ically, is defined to be the total moment acting on the n th body, about the n th frame origin, and 

expressed in the n th frame. If the external force acting on the n th body acts at some point other than the 
origin, as in Figure 1, the resulting moment due to the offset point of application ( r / x f n ) must be added 
to any pure external moment couple ( g %*) also acting on the individual body, in order to compute the total 
external moment about the n th body origin, G® xt . 
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Figure 2: Dynamic Simulation Procedure 


DYNAMIC SIMULATION 

The general procedure for simulating a spacecraft using momentum state dynamics involves at least one un- 
conventional step — the recovery of rates. Additionally, the requirements for a minimal spacecraft simulation 
are states representing the vehicle position (Rq), attitude (qj) and joint angles ( 6 ). Figure 2 depicts the 
procedure necessary to dynamically simulate the behavior of the multi-body system. 

An additional computation that may aid analysis during initialization and post-processing is determining 
velocity with respect to an inertial frame of the composite center of mass for the entire system (expressed 
in the “base” body-fixed frame). It may be calculated using the following relationship: 

Vcmo = V ° + | ' <*>0 C 0 + Ys “n A 0 k n c n j (43) 

Tracking the total kinetic energy of the system is also straightforward, given as KE — |V T MV. 

The dynamic formulation exists as an ANSI C code module in GSFC’s real-time Freespace simulation 
and analysis environment. As a validation and performance comparison, a version of the four-body model 
depicted in Figure 1 is tested against a similar model implemented in the commercial, general-purpose, 
multi-body simulation software package LMS/DADS© (v9.6). After one thousand seconds of simulated 
forced motion, the system rates (shown in Figure 3) differed by a maximum of 5 x 10” 6 . The system pa- 
rameters for the cross-validation test case are given in Table 2. Units are in MKS and degrees per second. 
Recall that I n is the second mass moment of inertia about the body center of mass, as opposed to j n that 
is about the body frame origin. All other configuration parameters not explicitly stated in Table 2 are zero. 


Table 2: Parameters for Test Simulation 


(Lx)o — 21261 7711,2,3 


^o, 1,2,3 

= 

[o 

0 

1] T 

b 0 ,i 

= 

[- 

1.524 0 2.5] 

bi,2 

= 

[5 

0 

0] T 

b2,3 

= 

[5 

0 

0] T 

c l, 2 

= 

[1250 

0 0] T 


(Ixrc 

)l, 2,3 

= 2.25 

q? 

(Iyy,zz)o 

= 30730 

q"+i 

■yy,zz 

)l, 2,3 

= 417.917 

LOt 


m 0 

= 10158 

ff‘ 


= 200 


= 

0 

0 

V2 

2 

V2' 

2 

T 

= [o 

0 

0 1 ] T 

n = 

= 1,2,3 

= - 

- 0.01 




= [0 

0.1 

0 ] T 




In addition executing more than four times quicker tnan the UAL)b^ tour- body model (which is to 
be expected since the present model is not a general purpose solver), there were also some alternate test 











time (sec) 


Figure 3: Validated Simulation Rates for Base Body with Three Links 


cases with higher initial body rates for which the DADS® solvers reported constraint tolerance violations — 
after which the two solutions diverged. Recalling some of the discussions in the introduction, specifically 
regarding general purpose solvers and the added complexity of the over-determined solution methods (i.e. 
ODE + DAE), it is likely that the present method is also more stable for this class of models. 


APPLICATION 

The dynamics model described was developed for the Hubble Robotic Servicing and Deorbit Mission’s 
(HRSDM) design, analysis and testing effort. The mission consisted of an autonomous grapple of the Hub- 
ble Space Telescope by a chaser vehicle using a six degrees-of-freedom robotic manipulator, similar to the 
RMS on the Space Shuttle (Figure 4). Additionally, post-grapple mission operations involved attaching a 
dexterous, two-armed robot (seven degrees-of-freedom per arm) to the end of the grapple arm and using the 
combined manipulators and tools to perform complex servicing operations. The design for the flight sys- 
tems (robots and controller) was the responsibility of McDonald-Detwiler Associates, Canada. However, the 
present model was implemented independently in a real-time dynamic simulation for validation and planning 
of the grapple and servicing. The open kinematic chain consisted of up to thirteen linkages connected in 
series with a non-flight joint controller for directing the robotic maneuvers. Some details of the manner in 
which a controller can benefit from the form of the dynamics will now be discussed. 


Joint Control 

The control laws used in the simulation to control the robot arms are restricted to joint-space control The 
calculated control is applied directly but with saturation levels. There are several control modes in the 
simulation and in each case, tne calculated torque is scaled by the outboard inertia about the axis of each 
joint. An actuator motor control torque is added to the joint dynamics so that Eq.(32) becomes: 
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Figure 4: Hubble Robotic Servicing and Deorbit Mission with Grapple Arm and Dexterous Robot 



&n ~ P n + a^gn + + 9n 

where < 7 “ is governed by a proportional, derivative and feedforward control law expressible as, 


(44) 


9 a n = 9n - m n ■ J a n (e n - e n ) - (K u ) n ■ j* K - O (45) 

Here, 6 n is the joint angle, ( Kq) u and (K U} ) n are the proportional and velocity control gains respectively, 
and refers to the composite second mass moment of inertia of the n th body and all bodies 

outboard, about the n th joint axis of rotation. This inertia term is clearly time-varying as the dynamics of 
the robot arm evolve. Note that for this controller, the gyroscopic coupling and external disturbances, such 
as gravity gradient, are not taken into account by the torque control law. At this point the control torque 
computed in Eq.(45) is sufficient since the motion of the grapple arm and/or dexterous arm is restricted by 
very low joint rate limits intended to keep base body disturbances small. The gyro scopic coupling terms in 
Eq.(44) are second order terms and are neglected. The gravity gradient terms are also very small and are 
effectively compensated for by the control torques. 


The terms, 6 n and u)£, denote desired or commanded quantities, and their determination depends 
on the control mode chosen for a particular task. For the HRSDM effort, five major control modes were 
implemented. They include: Static (Regulator) Control, Time- Optimal Path Control, Manual (man-in-the- 
loop) Control, Individual Joint Control, and Scripted Sequence Control. 


COMPUTATIONAL CONSIDERATIONS 
Efficiency 

In 1980, computational schemes for solving robot dynamics underwent a “quantum-leap” in efficiency with 
Lhc iiitiuuuetion of a recursive formulation tor me serial re volute manipulator by the MIT 1 team of Luh, 
Walker and Paul[10]. Their new algorithm for inverse dynamics relied on computations that grow linearly 
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with the number of links, commonly referred to as Order- TV or, more simply, 0 (TV) algorithms. Two years 
later, Walker and Orin[3] constructed an efficient forward dynamics solution using the inverse dynamics 
method of Luh et al[10]. Walker and Orin’s most efficient implementation (Method 3) exploited symmetry 
of the mass matrix and a recursive calculation of the mass properties to achieve solutions that were 100 
times more efficient [5] than the established Euler-Lagrange methods for N ~ 6. 

The Walker and Orin-Method 3 (WO-3) algorithm is, like the present momentum state dynamics (MSD) 
method, 0 (n 3 ) in complexity since both rely upon a numerical solution to a linear system of equations via 
Cholesky decomposition — which requires approximately | N 3 operations. Since the early 1980’s, there have 
been several advances in 0(iV)[7] and even 0 (log N) techniques. However, as shown in Featherstone[7], the 
0(n 3 ) algorithms still tend to be faster for N < 10, the class of problem in which most aerospace applications 
fall. As a result, the WO-3 benchmark is still a valid measure of a new formulation’s numerical efficiency. 
Table 3 provides a detailed comparison of the floating-point operations required for the two methods. Table 
4 is a more detailed profile of the present algorithm’s computational load. 


Table 3: Comparison of Number of Computations to Solve Forward Dynamics 


Method 

Multiplications 

Additions 

Bodies 

Links 

Mult 

Add 

WO-3 

±N 3 + 13 \N 2 + 192 \N - 49 

\N 3 + 8 \N 2 + 165|JV - 64 

6 

6 

1627 

1255 

MSD 

iJV 3 + 14JV 2 + 224AT - 30 

|W 3 + 11AT 2 + 175JV - 18 

6 

5 

1461 

1153 

7 

6 

1854 

1464 


There is some ambiguity in comparison to the classic Walker and Orin-Method 3 algorithm. The present 
method provides a coupled translational and rotational motion solution. In contrast, the WO-3 method is 
specific to a fixed-base system. The former is more amenable to spacecraft simulations, whereas the latter 
is specific to terrestrial robotics. Depending on whether the number of bodies (base + links) or just the 
number of links is counted for comparison, for AT = 6 the current method is either 10% faster or 15% slower 
than the WO-3 algorithm. Nevertheless, the momentum state dynamics method for serial manipulators is 
competitive with one of the best 0(AT 3 ) methods. 


From the algorithm profile in Table 4, it is clear 
that — apart from linear system solution — calculating 
the generalized mass matrix is the most computation- 
ally expensive operation. In fact, in order to improve 
the efficiency of the algorithm as N grows, additional 
intermediate quantities are introduced and a portion 
of the 0(W 2 ) operations were reformulated to have 
only 0(N) dependencies. Specifically when calculat- 
ing the “mixed” mass moments of inertia, both the 
N — 1 off-diagonal terms of the generalized mass ma- 


Calculation (13 links) 

millisecs 

% 

Mass Matrix Assembly 

0.042 

24% 

Linear System Solution 
(Cholesky Decomposition) 

0.111 

65% 

State Derivative Dynamics 

0.018 

11% 

Total 

0.171 

100% 


Table 4: Algorithm Component Run-times 
^ (MIPS R 16000 , 1 GHz Processor) 


trix (Jo 1 i,Ji I 2,...Jn,n+i) and those of rows 4-6 (Jo.i, Jo ,2 
are calculated explicitly using the second mass moment as given in Eq.(39). However, for the remaining set 
of “mixed” moments of inertia, whose members grow as |(AT — 2) (N — 1), two new recursion relationships 
are required as follows: 


Uz m = n-l 

m^n-l 


(46) 


to, __ /im+lT a i_X C' xa 

°m i n ~ n m u m,m+l K -'m>n 


(47) 
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where n = J m , n a n . Efficiency is gained in the G(N 2 ) operations by propagating 3x1 column matrices 
in the place of 3 x 3’s and also by eliminating many intermediate frame transformation constructions (e.g. 

. . .)• Another notable feature of Eqs.(46-47) is that the generalized mass matrix G(N 2 ) opera- 
tions are column-wise independent and are therefore candidates for parallel processing. 


Precision 

Two issues were discovered during the course of implementing the numerical simulation of the spacecraft 
and appendages. The first involves the propagation of the vehicle position and the second, round-off errors. 

Numerical integrators, such as an explicit Runge-Kutta method, propagate by perturbing the system 
states by relative amounts. If some states describe vehicle attitude (e.g. a quaternion that transforms from 
inertial to body coordinates) and they are perturbed, all quantities in body coordinates are also perturbed 
due to cross-coupling. While the present form of the equations requires that the dynamics be realized in 
body-fixed coordinates, experience has shown it is not beneficial to propagate the base-frame position states 
(R 0 ) in body coordinates as well. Instead, an inertial frame such as J2000 should be used. This prevents 
unnecessary step rejection due to the variations in attitude states coupling into perturbations of the orbit. 
By de-coupling the position and using double precision calculations, both an accurate attitude/ appendage 
dynamics solution and a stable orbit can be achieved for low earth orbiting (LEG) spacecraft without re- 
sorting to quadruple precision, non-dimensionalized variables[13] or Clohessy- Wiltshire approximations. 

The final numerical issue pertains to round-off precision. If one considers the equation for a single 
rigid-body spacecraft in LEO, the equations that propagate the angular momentum can be written as: 

H 0 = -WqH 0 - v 0 x P 0 + GJT (48) 

However, if the spacecraft mass on the order of 10 4 and the orbital velocity magnitude is of order 10 4 then 
a simple numerical experiment that calculates Vq MoVq using 16 significant figures of precision can yield 
round-off residuals with magnitudes of ~ 10 -5 , which may not be acceptable for a particular application. 
An alternate form of angular momentum propagation that explicitly cancels this term is: 

H 0 = — H 0 + v x C x u> 0 + GJT (49) 

which experience has shown to be well behaved for high-precision solution tolerances. 


CONCLUSIONS 


A new formulation is given for calculating the system dynamics of a free-floating “base” body with an 
attached appendage consisting of rigid linkages interconnected, chain-like, by a series of single degree-of- 
freedom rotational joints. Presented as an extension to the momentum state dynamics framework of P. C. 
Hughes [8], the present work adds robotic manipulators to the existing suite of multi-body models well suited 
for spacecraft applications such as reaction wheels, nutation dampers, control moment gyros, and fuel slosh. 
A compact and efficient recursive relationship allows for flexible simulation implementation of an arbitrary 
number of links in the chain and is suitable for real-time applications. 

The computational complexity of the solution method is (9(iV 3 ), and is shown to be competitive with the 
best algorithms of this class. However, opportunity exists for additional performance optimization such as 
using parallel computation methods for both the assembly of the generalized mass matrix and the solution 
of the linear set of momentum equations. Additional research might also suggest a method of avoiding the 
matrix decomposition altogether, yielding a lower order dependency on the number of bodies in the system. 


Finally, topics of future publications will most likely include extensions of the method to other system 
topologies (branches, closed-loops, geared joints, and link flexibility) as well as explication on the method 


fVio-f r>o t 




:cvcr jOIj.au ^onstraiiit o-aiu. liujineuts. 


12 



References 

[1] Uicker, J. J., “On the Dynamic Analysis of Spatial Linkages Using 4 by 4 Matrices”, Ph.D. Thesis, 
Department of Mechanical and Astronautical Sciences, Northwestern University, (1965). 

[2] Kane, T. R. and Levinson, D. A., “The Use of Kane’s Dynamical Equations in Robotics”, The Int 
Journal of Robotics Res . 2 , No. 3, pp. 3-21 (1983). 

[3] Walker, M. W., Orin, D. E., “Efficient Dynamic Computer Simulation of Robotic Mechanisms”, Journal 
of Dynamic Systems , Measurements , and Control , ASME, Vol. 104, pp. 205-211 (1982). 

[4] Silver, W. M., “On the Equivalence of Lagrangian and Newton-Euler Dynamics for Manipulators”, The 
Int Journal of Robotics Res . J No. 2, pp. 118-128 (1982). 

[5] Craig, J. J., Introduction to Robotics: Mechanics and Control , 2 nd ed., Addison- Wesley, (1989). 

[6] Shabana, A. A., Computational Dynamics , 2 nd ed., Wiley & Sons, (2001). 

[7] Featherstone, R., Robot Dynamics Algorithms , Kluwer, (1987). 

[8] Hughes, Peter C., Spacecraft Attitude Dynamics , Dover, (2004). 

[9] Ho, J. Y. L., “Direct Path Method for Flexible Multibody Spacecraft Dynamics”, Journal of Spacecraft 
and Rockets , Vol. 14, pp. 102-110 (1977). 

[10] Luh, J. Y. S., Walker, M. W., and Paul, R. P. C., “On Line Computational Scheme for Mechanical 
Manipulators”, Journal of Dynamic Systems , Measurements , and Control , ASME, Vol. 102, pp. 69-76 
(1980). 

[11] Ford, K. A., and Hall, C. D., “Flexible Spacecraft Reorientations Using Gimbaled Momentum Wheels”, 
The Journal of the Astronautical Sciences , ASME, Vol. 49, No. 3, pp. 421-441 (2001). 

[12] Denavit, J. and Hartenberg, R. S., “A Kinematic Notation for Lower-Pair Mechanisms Based on Ma- 
trices”, Journal of Applied Mechanics , June, pp. 215-221 (1955). 

[13] Santini, P. and Gasbarri, P., “Dynamics of Multibody System in Space Environment: Lagrangian vs. 
Eulerian Approach”, Acta Astronautica , No. 54, pp. 1-24 (2003). 


13 



